package org.noote.libs.computervision.detection.line;

import java.util.ArrayList;
import java.util.List;
import java.util.Random;

import org.noote.libs.computervision.math.Point2D;
import org.noote.libs.computervision.math.Vector2D;

// based on http://www.infi.nl/blog/view/id/56/Marker_Detection_for_Augmented_Reality_Applications

public class LineDetection_RANSAC extends LineDetection_Base {

	protected static final int _ransac_block = 48;
	protected static final int _ransac_step = 4;
	protected static final int _ransac_loop_tryout = 10;
	protected static final int _ransac_loop = 25;
	protected static final int _ransac_selected_min = 5;
	protected static final int _ransac_search_compatible_edgel_try = 50;
	protected static final boolean _ransac_expand_segments = false;
	
	private Random rnd = new Random();
	
	public class OrientedSegment extends Segment
	{
		public float orientation=0;
		List<Edgel> edgels = new ArrayList<Edgel>();
		
		public OrientedSegment(Point2D s, Point2D e)
		{
			super();
			set(s, e);
			orientation = calcul_orientation();
		}
		public OrientedSegment(Point2D s, Point2D e, float o)
		{
			super();
			set(s, e);
			orientation = o;
		}
		public OrientedSegment(Edgel es, Edgel ee)
		{
			super();
			set(es.position, ee.position);
			orientation = es.orientation;
			edgels.add(es);
			edgels.add(es);
		}
		public boolean isSupport(Edgel e)
		{
			// orientation compatible ?
			if(!isCompatibleOrientation(e.orientation))
				return false;

			if(getDistance(e.position)>0.75f)
				return false;

			return true;
		}
		
		public boolean isCompatible(Edgel e)
		{
			return isCompatibleOrientation(e.orientation);
		}
		public boolean isCompatibleOrientation(float o)
		{
			if(Math.abs(o-orientation) < (float)(Math.PI/8.0f))
				return true;

			return false;
		}
		@Override
		public float calcul_orientation()
		{
			if(edgels.size() == 0)
				return calcul_orientation_fast();
			
			float o = 0;
			for(Edgel e : edgels)
			{
				o += e.orientation;
			}
			
			return o/edgels.size();
		}
		public float calcul_orientation_fast()
		{
			return (float) Math.atan2(start.x-end.x, end.y-start.y);
		}
		@Override
		public void rebuild()
		{
			if(orientation>=(Math.PI*0.25f) && orientation<=(Math.PI*0.75f)) // down
			{
				float umin=_width, umax=0;
				for(Edgel e : edgels)
				{
					if(e.position.x<umin)
					{
						start = e.position;
						umin = start.x;
					}
					else if(e.position.x>umax)
					{
						end = e.position;
						umax = end.x;
					}
				}
				Point2D swap = start;	start = end; end = swap;
			}
			else if(orientation<=(-Math.PI*0.25f) && orientation>=(-Math.PI*0.75f)) // up
			{
				float umin=_width, umax=0;
				for(Edgel e : edgels)
				{
					if(e.position.x<umin)
					{
						start = e.position;
						umin = start.x;
					}
					else if(e.position.x>umax)
					{
						end = e.position;
						umax = end.x;
					}
				}
			}
			else if(orientation>=(-Math.PI*0.25f) && orientation<=(Math.PI*0.25f)) // right
			{
				float umin=_height, umax=0;
				for(Edgel e : edgels)
				{
					if(e.position.y<umin)
					{
						start = e.position;
						umin = start.y;
					}
					else if(e.position.y>umax)
					{
						end = e.position;
						umax = end.y;
					}
				}
			}
			else // if(orientation>(Math.PI*0.75f) || orientation<(-Math.PI*0.75f)) // left
			{
				float umin = _height, umax= 0;
				for(Edgel e : edgels)
				{
					if(e.position.y<umin)
					{
						start = e.position;
						umin = start.y;
					}
					else if(e.position.y>umax)
					{
						end = e.position;
						umax = end.y;
					}
				}
				Point2D swap = start;	start = end; end = swap;
			}
			calcul_orientation();
			
			// recalcul slope
			slope = new Vector2D(end.x - start.x, end.y - start.y);
			slope.normalize();
		}
		boolean isMergeable(OrientedSegment s, float fColinearDistanceMax, float fDistanceMax)
		{
			if(!isCompatibleOrientation(s.orientation))
				return false;
			
			// sur la meme ligne virtuel ?
			if(getDistance(s.start)>fColinearDistanceMax || getDistance(s.end)>fColinearDistanceMax)
				return false;

			// TODO : verifie si les limites sont compatible ?
	
			return true;
		}
		
		@Override
		public boolean expand(int bx, int by, int ex, int ey, float maxlen, boolean expand_start, boolean expand_end)
		{
			boolean bStartExpanded = false;
			boolean bEndExpanded = false;
			
			// continue from start
			float start_x = start.x;
			float start_y = start.y;
			float start_o = orientation;
			if(expand_start)
			{
				for(int s=0; s<maxlen; s++)
				{
					float t_start_x=start_x-slope.x;
					float t_start_y=start_y-slope.y;
					
					if(start_x<bx || start_y<by || start_x>ex || start_y>ey)
						break;
					
					int idx = (int)(start_y)*_width + (int)(start_x);				
					if(_iEdge[idx]<=_sobel_limit_min)
						break;
					
					if(!isCompatibleOrientation(_fOrientation[idx]))
						break;
					
					// point valid :)
					start_x = t_start_x;
					start_y = t_start_y;
					start_o = _fOrientation[idx];
					bStartExpanded = true;
				}
			}

			// continue from end
			float end_x = end.x;
			float end_y = end.y;
			float end_o = orientation;
			if(expand_end)
			{
				for(int e=0; e<maxlen; e++)
				{
					float t_end_x=end_x+slope.x;
					float t_end_y=end_y+slope.y;
					
					if(end_x<bx || end_y<by || end_x>ex || end_y>ey)
						break;
	
					int idx = (int)(end_y)*_width + (int)(end_x);				
					if(_iEdge[idx]<=_sobel_limit_min)
						break;
					
					if(!isCompatibleOrientation(_fOrientation[idx]))
						break;
					
					end_x = t_end_x;
					end_y = t_end_y;
					end_o = _fOrientation[idx];
					bEndExpanded = true;
				}
			}
			
			if(bStartExpanded || bEndExpanded)
			{
				if(bStartExpanded) edgels.add(new Edgel((int)start_x,(int)start_y, start_o));
				if(bEndExpanded) edgels.add(new Edgel((int)end_x,(int)end_y, end_o));
				rebuild();
			}
			
			return (bStartExpanded || bEndExpanded);
		}
	}
	public class Edgel
	{
		public Point2D position = new Point2D(0,0);
		public float orientation=0;
		
		public Edgel(int x, int y, float o) {
			position.x = x;
			position.y = y;
			orientation = o;
		}
		public Edgel(Point2D p, float o) {
			position = p;
			orientation = o;
		}
		boolean isCompatible(Edgel e)
		{
			return isCompatibleOrientation(e.orientation);
		}
		boolean isCompatibleOrientation(float o)
		{
			if(Math.abs(o-orientation) < (float)(Math.PI/8.0f))
				return true;

			return false;
		}
	}

	// some list
	public List<Edgel> _allransacedgels = new ArrayList<Edgel>();
	public List<OrientedSegment> _allransacsegments = new ArrayList<OrientedSegment>();
	
	protected void calcul_ransac()
	{
		// clear listes
		_allransacsegments.clear();
		_allransacedgels.clear();
		// decoupe en block de _ransac_block pour trouver les segments
		for(int ry=1; ry<_height-2; ry+=_ransac_block)
		{
			for(int rx=1; rx<_width-2; rx+=_ransac_block)
			{
				//  detect les edgels du block
				List<Edgel> edgels = new ArrayList<Edgel>();				
				for(int pyl=ry; pyl<(ry+_ransac_block); pyl++)
				{
					for(int pxl=rx; pxl<(rx+_ransac_block); pxl++)
					{
						int xy = pyl*_width+pxl;
							
						// take only 1/_ransac_step point (faster)
						if((pyl%_ransac_step == 0) && (pxl%_ransac_step) == 0
								&& pyl<_height-1 && pxl<_width-1)
						{
							// good visible edgel ?
							if(_iEdge[xy]>_sobel_limit_min)
							{
								Edgel e = new Edgel(pxl, pyl, _fOrientation[xy]);
								edgels.add(e);
								_allransacedgels.add(e);
							}
						}
					}
				}
				
				for(int lto=0; lto<_ransac_loop_tryout; lto++)
				{
					// si pas assez de edgels => on passe au block suivant
					int max_edgels = edgels.size();
					if(max_edgels < _ransac_selected_min)
						break;
			
					// recherche du segment le plus marquant du block
					OrientedSegment sel_line = null;
					for(int l=0; l<_ransac_loop; l++)
					{
						// do a virtual line with 2 random point
						// and find something "compatible"					
						Edgel es=null, ee=null;
						OrientedSegment line = null;
						for(int i=0; i<_ransac_search_compatible_edgel_try; i++)
						{
							// get two different points in edgels
							int ip1 = rnd.nextInt(max_edgels);
							int ip2 = rnd.nextInt(max_edgels);
							if(ip1 == ip2) continue;
							
							// get ours 2 edgels
							es = edgels.get(ip1);
							ee = edgels.get(ip2);
							
							// are theses edgels have a compatible orientation ?
							if(!es.isCompatible(ee)) continue;
							
							// build the line/segment between ip1 and ip2
							line = new OrientedSegment(es, ee);
							
							// does these line are compatible with selected point slope ?
							if(!line.isCompatible(ee)) continue;
							if(!line.isCompatible(es)) continue;
							
							// if I'm here : i have what i needed !
							break;
						}
						// finally... not edgels compatible ?!
						if(line == null)
							continue;
						
						// remove line edgels
						List<Edgel> edgels2 = new ArrayList<Edgel>();
						for(Edgel e : edgels)
						{
							if(e == es || e == ee)
								 continue;
							edgels2.add(e);
						}
						
						// check edgels in block 
						for(Edgel rr : edgels2)
						{
							// test if orientation point is near orientation line
							if(!line.isSupport(rr)) continue;
							
							// ok that's a good point
							line.edgels.add(rr);
						}
						if(line.edgels.size()>=_ransac_selected_min)
						{
							if(sel_line==null || line.edgels.size() > sel_line.edgels.size())
							{
								line.rebuild();
								
								sel_line = line;
							}
						}
					}
					if(sel_line != null)
					{
						if(_ransac_expand_segments)
							sel_line.expand(rx, ry, rx+_ransac_block, ry+_ransac_block, _ransac_block);
						_allransacsegments.add(sel_line);
						
						// remove sel_line.edgels from edgels.
						List<Edgel> new_edgels = new ArrayList<Edgel>();
						for(Edgel e : edgels)
						{
							boolean isIn = false;
							int jump = 0;
							for(Edgel se : sel_line.edgels)
							{
								if(jump%2==0
									&& e.position.x == se.position.x
									&& e.position.y == se.position.y)
								{
									isIn = true;
									break;
								}
								jump++;								
							}
							
							if(!isIn)
								new_edgels.add(e);
						}
						
						edgels = new_edgels;
					}
				}
			}
		}
	}
}
